#include "timer.h"

#include "stdio.h"
#include "stm32f4xx.h"

#include "RC.h"
#include "ANO_DT.h"

#include "Matek.h"
#include "mpu6050.h"
#include "motor.h"
#include "ak8975.h"
#include "HC_SR04.h"
#include "ks103.h"
#include "led.h"
#include "UWB.h"
#include "GPS.h"

#include "delay.h"
#include "usart.h"
#include "iic_moni.h"
#include "timer.h"

#include "Butterworth_filter.h"

#include "imu.h"
#include "IMU_CAM.h"
#include "Fusion.h"

#include "pid.h"
#include "ADRC.h"
#include "ppm.h"
#include "vector3.h"
#include "InertialNavigation.h"

#include "GradientDescent.h"

#include "Attitude.h"
#include "opticalflow.h" 

#include "Disturbance_observer.h"
#include "slidemode.h"

#include "control.h"

extern NDO High_NDO;
extern FLOAT_ANGLE Att_Angle,Angle_off;
extern FLOAT_XYZ Acc_filt,Gyr_rad,Magout;  
extern Light_Lidar Sys_Light_Lidar;
extern L0X3901_DataTypeDef LIDAR_DataStructure;
extern RC_Dat RC_Data;
extern yaw_control Yaw_control;
extern float N_AZ_CM,Ultra_V;
extern float yaw_off;

FLOAT_XYZ Magout;
uint8_t yaw_start = 0;
uint8_t xt = 0;

/*****************************************************************************
*函  数：void TIM2_Init(void)
*功  能：TIM2初始化为10ms计数一次
*参  数：无
*返回值：无
*备  注：更新中断时间 Tout = (ARR-1)*(PSC-1)/CK_INT
*****************************************************************************/
void TIM2_Init(void)
{
	TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;		

	/* 使能时钟 */
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2,ENABLE);

	TIM_DeInit(TIM2);

	/* 自动重装载寄存器周期的值(计数值) */
	TIM_TimeBaseStructure.TIM_Period = 10000 - 1;

	/* 累计 TIM_Period个频率后产生一个更新或者中断 */
	  /* 时钟预分频数为2 */
	TIM_TimeBaseStructure.TIM_Prescaler= 84 - 1;

	/* 对外部时钟进行采样的时钟分频,这里没有用到 */
	TIM_TimeBaseStructure.TIM_ClockDivision=TIM_CKD_DIV1;

	TIM_TimeBaseStructure.TIM_CounterMode=TIM_CounterMode_Up;   //向上计数

	TIM_TimeBaseInit(TIM2,&TIM_TimeBaseStructure);

	TIM_ClearFlag(TIM2,TIM_FLAG_Update);

	TIM_ITConfig(TIM2,TIM_IT_Update,ENABLE);
			
	TIM_Cmd(TIM2, DISABLE);																		
//	TIM_Cmd(TIM2, ENABLE);
 
}


uint32_t TIME_ISR_CNT=0;
uint8_t Height_mode;
/****************************************************************************************************
*函  数: void TIM2_IRQHandler(void) 
*功  能: TIM2定时器中断，10ms进一次中断也就是100Hz
*参  数: 无
*返回值: 无
*备  注: 此函数是整个程序的运行时基，不同的中断时间对应不同频率，
*        对于一些计算对调用时间要求比较严格时可用此方法；
*        扫描频率 = 100Hz/分频系数；
****************************************************************************************************/

void TIM2_IRQHandler(void) 
{
	static uint8_t ms10 = 0; 
	static uint8_t ms20 = 0; 
	static uint8_t ms50 = 0;
	static uint8_t ms100 = 0;
    static uint16_t ms5000 = 0;	
	if(TIM_GetITStatus(TIM2,TIM_IT_Update) != RESET)
	{
		TIME_ISR_CNT++;
		ms10++;
		ms20++;
		ms50++;
		ms100++;
		
		if(ms20>=2)
		{
			ms20 = 0;           //通过改变采样间隔滤，因为跳变不止持续一个周期，这样就能滤掉
			RC_Data_Trs();
		}
		
		if(ms50==5)
		{
			ms50 = 0;           //通过改变采样间隔滤，因为跳变不止持续一个周期，这样就能滤掉
			Remote_DataFilter();//遥控滤波检测跳变，这里的值50ms一次。滤除锁定键 模式键的异常数据		
			Task_led();
			//UltraSonic_KS103_Feed();
		}
				
		IMU_Data_PLUS();
		Magout = AK8975_Getdata(); 
				
		Compute_HorizontalXY();
		Navigation_DataPro();
		
		#if defined (Ultrasonic_OD)
		//	HC_SR04_Read();
			Height_mode = Ultrasonic_OD;
		
		#elif defined (Laser_TOF)
			Height_mode = Laser_TOF;
			
		#endif
		
		UltraSonic_US100_Feed();
				
		Altitude_ComFusion(Height_mode);
		
		OptFlow_Read();//光流传感器
		UWB_FuseData();
		
		READ_GPS_DATA();
		
		//Control();
		Master_Control();
		//BaseState_NDO_RUN(&High_NDO,N_AZ_CM,Ultra_V,0.013);
		//Slidemode_Control();
		
		if(ms100==10)  //10Hz
		{ 
			ms100 = 0; 
			if(RC_Data.Accoffset_Flag ==1)  //加速度六面校准，八字校准
			{ 
				LED_OFF();
				Acc_sixcam(); 
			}
		}  
		
		
	    if(yaw_start==0)  //只执行一次，在上电约5S后确定
	    {		
		   ms5000++;
		   if(ms5000>=500)
		   {  
			  yaw_start = 1; 
			  Yaw_control.Ready = 1; 
			  Yaw_control.angletarget = Att_Angle.yaw;
			  yaw_off = Att_Angle.yaw;
			  LEDflash(0);  //白色灯闪烁代
			   
			  xt = 1;		 
			  Gyro_avercam();
		   }
	    } 
			

	}
	TIM_ClearITPendingBit(TIM2,TIM_IT_Update);
}


/****************************************************************************************************
*函数: TIM5_IRQHandler(void)
*功能: 定时器5中断，每5ms
*参数: 无
*返回值:无
*备注: 无
****************************************************************************************************/
void TIM5_IRQHandler(void)//10ms
{
  if( TIM_GetITStatus(TIM5,TIM_IT_Update)!=RESET )
  {
	  //大约消耗0.2ms就不得了
	AHRS_compute(&Acc_filt,&Gyr_rad,&Magout,&Att_Angle);	
	Angle_Suboffset(&Angle_off,&Att_Angle);
	  
	Att_inner_Ctrl();
    Motor_Control();	
	  
    TIM_ClearITPendingBit(TIM5,TIM_FLAG_Update);
      
  }
  
}

